In [1]:
import krpc
In [2]:
import numpy as np
import time
This is a simple launch. No reference to geo-position in control loop. Roll and yaw will be fixed at zero. Pitch will start at 90 deg until 60 m/s at which point we will pitch towards 30 deg above the eastern horizon at flame-out.
This is a booster-only single-body rocket with two stages: launch and parachute.
In [3]:
linkup = krpc.connect('192.168.1.2', name='First Flight')
In [4]:
import numpy as np
from matplotlib import pyplot, cm
inf = np.inf
isclose = np.isclose
π = np.pi
arctan = np.arctan
sign = np.sign
In [5]:
class Controller(object):
'''Single Axis PID Controller'''
def __init__(self,set_point=0,limits=(-inf,inf),kp=1,ki=0,kd=0,t0=0):
# set point and control constants
self.set_point = set_point
self.min,self.max = limits
self.kp = kp
self.ki = ki
self.kd = kd
# time of previous call, running integral and
# proportional term of previous call
self.t0 = t0
self.I = 0
self.P0 = 0
# response value of previous call
self.c = 0
def __call__(self,x,t):
# return previous value if no time has passed
if isclose(t - self.t0, 0):
return self.c
# bring instance variables into local scope
xset = self.set_point
kp = self.kp
ki = self.ki
kd = self.kd
# if parameters are all zero or None, return set point
if not any([kp,ki,kd]):
self.t0 = t
return xset
# bring instance variables into local scope
t0 = self.t0
I = self.I
P0 = self.P0
# calculate PID terms
Δt = t - t0
P = xset - x
ΔP = P - P0
D = ΔP / Δt
# freeze integral for a small time on
# a large disturbance
if self.ki > 0:
if abs(kp*ΔP) > 0.5*(self.max - self.min):
self._t0_freeze_I = t
else:
try:
if (t - self._t0_freeze_I) > self.ti:
del self._t0_freeze_I
I += P * Δt
except AttributeError:
I += P * Δt
# turn off integral term if kp*P is out of the
# control range
if not (self.min < kp*P < self.max):
I = 0
else:
I = min(max(I, self.min/ki), self.max/ki)
# clip proportional gain
if not (self.min < kp*P < self.max):
P = min(max(P, self.min/kp), self.max/kp)
c = kp*P + ki*I + kd*D
# clip output to specified limits
c = min(max(c, self.min), self.max)
# save parameters to class instance
self.t0 = t
self.I = I
self.P0 = P
self.c = c
return c
@property
def ti(self):
'''integral time'''
return self.kp / self.ki
@ti.setter
def ti(self,ti):
self.ki = self.kp / ti
@property
def td(self):
'''derivative time'''
return self.kd / self.kp
@td.setter
def td(self,td):
self.kd = self.kp * td
@property
def ku(self):
'''ultimate gain, assuming classic ziegler-nichols pid scheme'''
return (1/.6)*self.kp
@ku.setter
def ku(self,ku):
self.kp = .6*ku
@property
def tu(self):
'''period of oscillation at ultimate gain'''
return 2*self.kp/self.ki
@tu.setter
def tu(self,tu):
self.ki = 2*self.kp/tu
self.kd = self.kp*tu/8
def ziegler_nichols(self,ku,tu,control_type='pid'):
'''
ku = ultimate gain
tu = period of oscillation at ultimate gain
'''
converter = dict(
p = lambda ku,tu: (.5*ku, 0, 0),
pi = lambda ku,tu: (.45*ku, 1.2*(.45*ku)/tu, 0),
pd = lambda ku,tu: (.8*ku, 0, (.8*ku)*tu/8),
pid = lambda ku,tu: (.6*ku, 2*(.6*ku)/tu, (.6*ku)*tu/8),
pessen = lambda ku,tu: (.7*ku, 2.5*(.7*ku)/tu, 3*(.7*ku)*tu/20),
some_overshoot = lambda ku,tu: (.33*ku, 2*(.33*ku)/tu, (.33*ku)*tu/3),
no_overshoot = lambda ku,tu: (.2*ku, 2*(.2*ku)/tu, (.2*ku)*tu/3)
)
self.kp,self.ki,self.kd = converter[control_type.lower()](ku,tu)
In [6]:
def main(linkup):
ksc = linkup.space_center
vessel = ksc.active_vessel
body = vessel.orbit.body
altitude = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'mean_altitude')
vertical_speed = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'vertical_speed')
speed = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'speed')
pitch = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'pitch')
heading = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'heading')
roll = linkup.add_stream(getattr, vessel.flight(body.reference_frame), 'roll')
con = vessel.control
experiments = {'goo' : vessel.parts.with_name('GooExperiment')}
capsule = vessel.parts.with_name('mk1pod')[0]
def crew_report(capsule):
report_module = capsule.module[2]
report_action = report_module.actions[0]
report_module.trigger_event(report_action)
def observe_goo(goo_experiment):
goo_module = goo_experiment.modules[1]
observe_action = goo_module.actions[0]
goo_module.trigger_event(observe_action)
t0 = ksc.ut
pitch_con = Controller(set_point=pitch()*π/180,limits=(-1,1),kp=1,ki=0.6,kd=1,t0=t0)
heading_con = Controller(set_point=heading()*π/180,limits=(-1,1),kp=1,ki=0.6,kd=1,t0=t0)
roll_con = Controller(set_point=roll()*π/180,limits=(-1,1),kp=1,ki=0.6,kd=1,t0=t0)
observe_goo(experiments['goo'][0])
time.sleep(3)
con.activate_next_stage() # launch!
while speed() < 60:
t = ksc.ut
con.yaw = pitch_con(pitch()*π/180, t)
con.pitch = heading_con(heading()*π/180, t)
con.roll = roll_con(roll()*π/180, t)
time.sleep(0.001)
ftot = vessel.resources.amount('SolidFuel')
frem = ftot
while frem > 0.1:
frem = vessel.resources.amount('SolidFuel')
pitch_con.set_point = 15 - (15 - 50) * (ftot - frem) / ftot
t = ksc.ut
con.yaw = pitch_con(pitch()*π/180, t)
con.pitch = heading_con(heading()*π/180, t)
con.roll = roll_con(roll()*π/180, t)
time.sleep(0.001)
reported = False
while altitude() > 8000:
if not reported and vertical_speed() < 0:
observe_goo(experiments['goo'][1])
crew_report(capsule)
reported = True
time.sleep(1)
con.activate_next_stage() # parachutes
while vessel.situation is not splashed:
time.sleep(1)
observe_goo(experiments['goo'][2])
In [7]:
main(linkup)
In [25]:
moi = np.array(vessel.moment_of_inertia)
itensor = np.array(vessel.inertia_tensor).reshape(3,3)
availtorque = np.array(vessel.available_torque)
print('''
moi: {moi}
itensor: {itensor}
availtorque: {availtorque}
'''.format(moi=moi, itensor=itensor, availtorque=availtorque))
Given a moment of intertia vector around (pitch, roll, yaw) in kg m^2 and an available torque vector around (pitch, roll, yaw) in N m or kg m^2 / s^2, I want to create a controller (presumably PID) in the range [-1,1] for each axis.
In [29]:
moment_of_inertia = 1733
max_torque = 5000
control_range = np.array([-1,1])
max_acceleration = max_torque * control_range[1] / moment_of_inertia
print('max accel:',max_acceleration,'rad / s^2')
print(max_acceleration*180/np.pi, 'deg / s^2')
This is very fast. I only want a couple degrees per second^2 acceleration and not more than about 5 deg/sec rotation ever, so let's see what control range that represents:
In [30]:
print('control range at 2 deg / s^2:',(2*np.pi/180) * moment_of_inertia / max_torque)
This might be a good limit on the control rate of change.
The output would be for a device 2 deg from the set point at 2 deg/sec with zero windup: $$output = Kp * 2 + Kd * 2$$